#include "spi.h"
#include "BM1088.h"
#include "gpio.h"

#define ENABLE_ACEEL	{HAL_GPIO_WritePin(CS1_Accel_GPIO_Port,CS1_Accel_Pin,GPIO_PIN_RESET);HAL_GPIO_WritePin(CS1_Gyro_GPIO_Port,CS1_Gyro_Pin,GPIO_PIN_SET);}
#define ENABLE_GYRO		{HAL_GPIO_WritePin(CS1_Accel_GPIO_Port,CS1_Accel_Pin,GPIO_PIN_SET);HAL_GPIO_WritePin(CS1_Gyro_GPIO_Port,CS1_Gyro_Pin,GPIO_PIN_RESET);}
#define DISABLE_ACEEL	{HAL_GPIO_WritePin(CS1_Accel_GPIO_Port,CS1_Accel_Pin,GPIO_PIN_SET);HAL_GPIO_WritePin(CS1_Gyro_GPIO_Port,CS1_Gyro_Pin,GPIO_PIN_SET);}
#define DISABLE_GYRO	{HAL_GPIO_WritePin(CS1_Accel_GPIO_Port,CS1_Accel_Pin,GPIO_PIN_SET);HAL_GPIO_WritePin(CS1_Gyro_GPIO_Port,CS1_Gyro_Pin,GPIO_PIN_SET);}
#define BM1088_SPI		 hspi1

extern float acc_x_float,acc_y_float,acc_z_float,gyro_x_float,gyro_y_float,gyro_z_float;
extern uint8_t float_readcomplete;
void write_data_to_gyro(uint8_t addr,uint8_t data)
{
	ENABLE_GYRO;
	uint8_t tmp=0x7f;//01111111
	uint8_t sending_data=tmp&addr;
	
	HAL_SPI_Transmit(&BM1088_SPI,&sending_data,1,1000);
	while(HAL_SPI_GetState(&BM1088_SPI)==HAL_SPI_STATE_BUSY_TX)
		;
	
	sending_data=data;
	
	HAL_SPI_Transmit(&BM1088_SPI,&sending_data,1,1000);
	while(HAL_SPI_GetState(&BM1088_SPI)==HAL_SPI_STATE_BUSY_TX)
		;
	
	HAL_Delay(1);
	DISABLE_GYRO;
}

void read_data_from_gyro(uint8_t addr,uint8_t *data)
{
	ENABLE_GYRO;
	uint8_t tmp=0x80;//10000000
	uint8_t sending_data=tmp|addr;
	HAL_SPI_Transmit(&BM1088_SPI,&sending_data,1,1000);
	while(HAL_SPI_GetState(&BM1088_SPI)==HAL_SPI_STATE_BUSY_TX)
		;
	
	HAL_SPI_Receive(&BM1088_SPI, data, 1, 1000);
	while(HAL_SPI_GetState(&BM1088_SPI)==HAL_SPI_STATE_BUSY_RX)
		;
	
	DISABLE_GYRO;
}

void read_multi_data_from_gyro(uint8_t addr,uint8_t len,uint8_t *data)
{
	ENABLE_GYRO;
	uint8_t tmp=0x80;//10000000
	uint8_t sending_data=tmp|addr;
	uint8_t receive_data;
	HAL_SPI_Transmit(&BM1088_SPI,&sending_data,1,1000);
	while(HAL_SPI_GetState(&BM1088_SPI)==HAL_SPI_STATE_BUSY_TX)
		;
	
	for(int i=0;i<len;i++)
	{
		HAL_SPI_Receive(&BM1088_SPI, &receive_data, 1, 1000);
		while(HAL_SPI_GetState(&BM1088_SPI)==HAL_SPI_STATE_BUSY_RX)
			;
		data[i] = receive_data;
	}
	
	
	DISABLE_GYRO;
}

void write_data_to_accel(uint8_t addr,uint8_t data)
{
	ENABLE_ACEEL;
	uint8_t tmp=0x7f;//01111111
	uint8_t sending_data=tmp&addr;
	
	HAL_SPI_Transmit(&BM1088_SPI,&sending_data,1,1000);
	while(HAL_SPI_GetState(&BM1088_SPI)==HAL_SPI_STATE_BUSY_TX)
		;
	
	sending_data=data;
	
	HAL_SPI_Transmit(&BM1088_SPI,&sending_data,1,1000);
	while(HAL_SPI_GetState(&BM1088_SPI)==HAL_SPI_STATE_BUSY_TX)
		;
	
	HAL_Delay(1);
	DISABLE_ACEEL;
}

void read_data_from_accel(uint8_t addr,uint8_t *data)
{
	ENABLE_ACEEL;
	uint8_t tmp=0x80;//10000000
	uint8_t sending_data=tmp|addr;
	HAL_SPI_Transmit(&BM1088_SPI,&sending_data,1,1000);
	while(HAL_SPI_GetState(&BM1088_SPI)==HAL_SPI_STATE_BUSY_TX)
		;
	
	HAL_SPI_Receive(&BM1088_SPI, data, 1, 1000);
	while(HAL_SPI_GetState(&BM1088_SPI)==HAL_SPI_STATE_BUSY_RX)
		;

	HAL_SPI_Receive(&BM1088_SPI, data, 1, 1000);
	while(HAL_SPI_GetState(&BM1088_SPI)==HAL_SPI_STATE_BUSY_RX)
		;
	
	DISABLE_ACEEL;
}

void read_multi_data_from_accel(uint8_t addr,uint8_t len,uint8_t *data)
{
	ENABLE_ACEEL;
	uint8_t tmp=0x80;//10000000
	uint8_t sending_data=tmp|addr;
	uint8_t receive_data;
	HAL_SPI_Transmit(&BM1088_SPI,&sending_data,1,1000);
	while(HAL_SPI_GetState(&BM1088_SPI)==HAL_SPI_STATE_BUSY_TX)
		;
	
	HAL_SPI_Receive(&BM1088_SPI, &receive_data, 1, 1000);
	while(HAL_SPI_GetState(&BM1088_SPI)==HAL_SPI_STATE_BUSY_RX)
		;
	
	for(int i=0;i<len;i++)
	{
		HAL_SPI_Receive(&BM1088_SPI, &receive_data, 1, 1000);
		while(HAL_SPI_GetState(&BM1088_SPI)==HAL_SPI_STATE_BUSY_RX)
			;
		data[i] = receive_data;
	}
	
	
	DISABLE_ACEEL;
}	

void BM1088_init(void)
{
    // 加速度计初始化
    // 先软重启，清空所有寄存器
    write_data_to_accel(ACC_SOFTRESET_ADDR, ACC_SOFTRESET_VAL);
    HAL_Delay(50);
    // 打开加速度计电源
    write_data_to_accel(ACC_PWR_CTRL_ADDR, ACC_PWR_CTRL_ON);
    // 加速度计变成正常模式
    write_data_to_accel(ACC_PWR_CONF_ADDR, ACC_PWR_CONF_ACT);

    // 陀螺仪初始化
    // 先软重启，清空所有寄存器
    write_data_to_gyro(GYRO_SOFTRESET_ADDR, GYRO_SOFTRESET_VAL);
    HAL_Delay(50);
    // 陀螺仪变成正常模式
    write_data_to_gyro(GYRO_LPM1_ADDR, GYRO_LPM1_NOR);

    // 加速度计配置写入
    // 写入范围，+-3g的测量范围
    write_data_to_accel(ACC_RANGE_ADDR, ACC_RANGE_3G);
    // 写入配置，正常带宽，1600hz输出频率
    write_data_to_accel(ACC_CONF_ADDR,
                   (ACC_CONF_RESERVED << 7) | (ACC_CONF_BWP_NORM << 6) | (ACC_CONF_ODR_1600_Hz));

    // 陀螺仪配置写入
    // 写入范围，+-500°/s的测量范围
    write_data_to_gyro(GYRO_RANGE_ADDR, GYRO_RANGE_500_DEG_S);
    // 写入带宽，2000Hz输出频率，532Hz滤波器带宽
    write_data_to_gyro(GYRO_BANDWIDTH_ADDR, GYRO_ODR_2000Hz_BANDWIDTH_532Hz);
	
}

void ReadAccData(struct BM1088_acc_data *data) {
    uint8_t buf[ACC_XYZ_LEN], range;
    int16_t acc[3];
    read_data_from_accel(ACC_RANGE_ADDR, &range);
    read_multi_data_from_accel(ACC_X_LSB_ADDR, ACC_XYZ_LEN, buf);
    acc[0] = ((int16_t)buf[1] << 8) + (int16_t)buf[0];
    acc[1] = ((int16_t)buf[3] << 8) + (int16_t)buf[2];
    acc[2] = ((int16_t)buf[5] << 8) + (int16_t)buf[4];
    data->acc_x = (float)acc[0] * BMI088_ACCEL_3G_SEN;
    data->acc_y = (float)acc[1] * BMI088_ACCEL_3G_SEN;
    data->acc_z = (float)acc[2] * BMI088_ACCEL_3G_SEN;
		if(float_readcomplete==1)
		{
			data->acc_x-=acc_x_float;
			data->acc_y-=acc_y_float;
			data->acc_z-=acc_z_float;
		}
}

void ReadGyroData(struct BM1088_gyro_data *data) {
    uint8_t buf[GYRO_XYZ_LEN], range;
    int16_t gyro[3];
    float unit;
    read_data_from_gyro(GYRO_RANGE_ADDR, &range);
    switch (range) {
        case 0x00:
            unit = 16.384;
            break;
        case 0x01:
            unit = 32.768;
            break;
        case 0x02:
            unit = 65.536;
            break;
        case 0x03:
            unit = 131.072;
            break;
        case 0x04:
            unit = 262.144;
            break;
        default:
            unit = 16.384;
            break;
    }
    read_multi_data_from_gyro(GYRO_RATE_X_LSB_ADDR, GYRO_XYZ_LEN, buf);
    gyro[0] = ((int16_t)buf[1] << 8) + (int16_t)buf[0];
    gyro[1] = ((int16_t)buf[3] << 8) + (int16_t)buf[2];
    gyro[2] = ((int16_t)buf[5] << 8) + (int16_t)buf[4];
    data->gyro_roll = (float)gyro[0] / unit * DEG2SEC;
    data->gyro_pitch = (float)gyro[1] / unit * DEG2SEC;
    data->gyro_yaw = (float)gyro[2] / unit * DEG2SEC;
		if(float_readcomplete==1)
		{
			data->gyro_roll-=gyro_x_float;
			data->gyro_pitch-=gyro_y_float;
			data->gyro_yaw-=gyro_z_float;
		}

}

void CalculateFloat(struct BM1088_acc_data *acc_data,struct BM1088_gyro_data *gyro_data)
{
	for(int i=0;i<1000;i++)
	{
		ReadAccData(acc_data);
		ReadGyroData(gyro_data);
		acc_x_float+=acc_data->acc_x;
		acc_y_float+=acc_data->acc_y;
		//acc_z_float+=acc_data->acc_z;
		gyro_x_float+=gyro_data->gyro_pitch;
		gyro_y_float+=gyro_data->gyro_roll;
		gyro_z_float+=gyro_data->gyro_yaw;		
	}
	acc_x_float/=1000.0;
	acc_y_float/=1000.0;
	//acc_z_float/=1000.0;
	gyro_x_float/=1000.0;
	gyro_y_float/=1000.0;
	gyro_z_float/=1000.0;
	float_readcomplete=1;
}

void ReadAccSensorTime(float *time) {
    uint8_t buf[SENSORTIME_LEN];
    read_multi_data_from_accel(SENSORTIME_0_ADDR, SENSORTIME_LEN, buf);
    *time = buf[0] * SENSORTIME_0_UNIT + buf[1] * SENSORTIME_1_UNIT + buf[2] * SENSORTIME_2_UNIT;
}

void ReadAccTemperature(float *temp) {
    uint8_t buf[TEMP_LEN];
    read_multi_data_from_accel(TEMP_MSB_ADDR, TEMP_LEN, buf);
    uint16_t temp_uint11 = (buf[0] << 3) + (buf[1] >> 5);
    int16_t temp_int11;
    if (temp_uint11 > 1023) {
        temp_int11 = (int16_t)temp_uint11 - 2048;
    } else {
        temp_int11 = (int16_t)temp_uint11;
    }
    *temp = temp_int11 * TEMP_UNIT + TEMP_BIAS;
}